#include <chrono>
#include <memory>
#include <thread>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>

class TestImuPublisher : public rclcpp::Node {
public:
    TestImuPublisher() : Node("test_imu_publisher") {
        // 创建发布者
        imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu/data", 10);
        mag_pub_ = this->create_publisher<sensor_msgs::msg::MagneticField>("/imu/mag", 10);

        // 创建定时器
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(10),  // 100Hz
            std::bind(&TestImuPublisher::timer_callback, this));

        RCLCPP_INFO(this->get_logger(), "测试IMU发布者已启动，模拟数据发布中...");
    }

private:
    void timer_callback() {
        auto now = this->get_clock()->now();
        
        // 创建IMU消息
        auto imu_msg = sensor_msgs::msg::Imu();
        imu_msg.header.stamp = now;
        imu_msg.header.frame_id = "imu_link";

        // 模拟数据
        double t = now.seconds();
        
        // 模拟线加速度（包含重力）
        imu_msg.linear_acceleration.x = 0.1 * sin(t);
        imu_msg.linear_acceleration.y = 0.2 * cos(t);
        imu_msg.linear_acceleration.z = 9.8 + 0.05 * sin(2 * t);  // 重力 + 小幅振动

        // 模拟角速度
        imu_msg.angular_velocity.x = 0.01 * sin(0.5 * t);
        imu_msg.angular_velocity.y = 0.02 * cos(0.3 * t);
        imu_msg.angular_velocity.z = 0.005 * sin(0.1 * t);

        // 模拟四元数（轻微旋转）
        imu_msg.orientation.w = 1.0;
        imu_msg.orientation.x = 0.01 * sin(t * 0.1);
        imu_msg.orientation.y = 0.01 * cos(t * 0.1);
        imu_msg.orientation.z = 0.005 * sin(t * 0.05);

        imu_pub_->publish(imu_msg);

        // 每隔一段时间发布磁场数据
        if (static_cast<int>(t * 10) % 50 == 0) {  // 每5秒发布一次
            auto mag_msg = sensor_msgs::msg::MagneticField();
            mag_msg.header.stamp = now;
            mag_msg.header.frame_id = "imu_link";
            
            // 模拟磁场数据
            mag_msg.magnetic_field.x = 2.5e-5 + 1e-6 * sin(t);
            mag_msg.magnetic_field.y = 1.2e-5 + 0.5e-6 * cos(t);
            mag_msg.magnetic_field.z = 4.2e-5 + 0.8e-6 * sin(t * 1.5);
            
            mag_pub_->publish(mag_msg);
        }
    }

    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
    rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr mag_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    
    auto node = std::make_shared<TestImuPublisher>();
    rclcpp::spin(node);
    
    rclcpp::shutdown();
    return 0;
}
